/*****************************************************************************************************
*       SRLOS Team
*@filename:     k_cp15.c
*@brief   :     this file defines some type and macros for cp15 using
*@author  :     bloceanc
*@note    :     we need some asm function codes
*@comment :
*@datetime:
*****************************************************************************************************/
#ifndef _K_CP15_C_
#define _K_CP15_C_

/**
 *	 Technical Releations in CP15 register
 *	1.access CP15 instructions:
 * 		MCR{cond} p15,<Opcode_1>,<Rd>,<CRn>,<CRm>,<Opcode_2>	WRITE CP15
 *		MRC{cond} p15,<Opcode_1>,<Rd>,<CRn>,<CRm>,<Opcode_2>	READ CP15
 *
 * 	For more informations, to see <arm926ej-s Technical Reference Manual>
 */
#define K_CP15_ASM_READ(__val__, __asm_code__)	\
{	\
	__asm__("STMFD R13!, {R0}\n\t"	\
	__asm_code__	\
	"STR R0, %0\n\t"				\
	"LDMFD R13!, {R0}\n\t"			\
	:"=m"(__val__));				\
}

/****************************************************************************************/
/*                                 CP15 Register 0 Operations                           */
/* Register c0 accesses the ID Register, Cache Type Register, and TCM Status Registers.**/
/****************************************************************************************/
/**
 * @BRIEF	: Read CP15 c0 register ID code
 * @PARAM	: void
 * @PARAM	: void
 * @RETRUN	: ID code
 */
unsigned int k_cp15_c0_read_id_code(void)
{
	unsigned int val;
	K_CP15_ASM_READ(val, "MRC p15, 0, R0, c0, c0, 0\n\t");
//	__asm__("STMFD R13!, {R0}\n\t"	\
//	"MRC p15, 0, R0, c0, c0, 0\n\t"	\
//	"STR R0, %0\n\t"				\
//	"LDMFD R13!, {R0}\n\t"			\
//	:"=m"(val));
	return val;
}

/**
 * @BRIEF	: Read CP15 c0 register Cache type
 * @PARAM	: void
 * @PARAM	: void
 * @RETRUN	: Cache type
 */
unsigned int k_cp15_c0_read_cache_type(void)
{
	unsigned int val;
	K_CP15_ASM_READ(val, "MRC p15,0,R0,c0,c0,1\n\t");
//	__asm__("MRC p15,0,%0,c0,c0,1\n\t":"=m"(val));
	return val;
}

/**
 * @BRIEF	: Read CP15 c0 register TCM status
 * @PARAM	: void
 * @PARAM	: void
 * @RETRUN	: TCM status
 */
unsigned int k_cp15_c0_read_tcm_status(void)
{
	unsigned int val;
	K_CP15_ASM_READ(val, "MRC p15,0,R0,c0,c0,2\n\t");
//	__asm__("MRC p15,0,%0,c0,c0,2\n\t":"=m"(val));
	return val;
}

/****************************************************************************************/
/*                                 CP15 Register 1 Operations                           */
/*
		This register specifies the configuration used to enable and disable the caches
	and MMU. It is recommended that you access this register using a read-modify-write
	sequence.
*/
/****************************************************************************************/
// MRC p15, 0, <Rd>, c1, c0, 0 ; read control register
// MCR p15, 0, <Rd>, c1, c0, 0 ; write control register

// replacement strategy for ICache and DCache
#define K_CP15_C1_REPLACE_RAND	0xFFFFBFFF		// Random Replacement
#define K_CP15_C1_REPLACE_ROBIN	0x4000			// Round-robin replacement

/* Operation macros for set CP15 C1 register */
// ORR operation!(Set flag to 1)
#define K_CP15_C1_OP_SET(__val__)	{	\
	__asm__(	\
	"STMFD R13!, {R0}\n\t"	\
	"MRC p15, 0, R0, c1, c0, 0\n\t"	\
	"ORR R0, R0, %0\n\t"	\
	"MCR p15, 0, R0, c1, c0, 0\n\t"	\
	"LDMFD R13!, {R0}\n\t"	\
	::"r"(__val__));	\
}

// ADD operation(Set Flag to 0)
#define K_CP15_C1_OP_CLEAR(__val__)	{	\
	__asm__(	\
	"STMFD R13!, {R0}\n\t"	\
	"MRC p15, 0, R0, c1, c0, 0\n\t"	\
	"ADD R0, R0, %0\n\t"	\
	"MCR p15, 0, R0, c1, c0, 0\n\t"	\
	"LDMFD R13!, {R0}\n\t"	\
	::"r"(__val__));	\
}


/**
 * @BRIEF	: Modify using Random strategy on ICache and DCache
 * @PARAM	: void
 * @RETRUN	: void
 */
void k_cp15_c1_replace_random(void)
{
	K_CP15_C1_OP_CLEAR(K_CP15_C1_REPLACE_RAND);
}

/**
 * @BRIEF	: Modify using Round-Robin strategy on ICache and DCache
 * @PARAM	: void
 * @RETRUN	: void
 */
void k_cp15_c1_replace_robin(void)
{
	K_CP15_C1_OP_SET(K_CP15_C1_REPLACE_ROBIN);
}

/**
 * @BRIEF	: Set Location of exception vectors in system
 * @PARAM	: loaction		0x0000_0000  or 0xffff_0000
 * @RETRUN	: void
 */
void k_cp15_c1_set_exception_location(unsigned int location)
{
	if(location == 0x0)
	{
		K_CP15_C1_OP_CLEAR(0xffffdfff);
	}
	else
	{
		K_CP15_C1_OP_SET(0x2000);
	}
}

/**
 * @BRIEF	: Enable/Disable ICache(Instructions Cache)
 * @PARAM	: b			0 disable; 1 enable
 * @RETRUN	: void
 */
void k_cp5_c1_ICache_enablish(unsigned int b)
{
	if(b)
	{
		K_CP15_C1_OP_SET(0x1000);
	}
	else
	{
		K_CP15_C1_OP_CLEAR(0xffffefff)
	}
}

/**
 * @BRIEF	: Set endian of processor
 * @PARAM	: b			0 big-endian; 1 little-endian
 * @RETRUN	: void
 */
void k_cp15_c1_set_endian(unsigned int b)
{
	if(b)
	{
		K_CP15_C1_OP_CLEAR(0xffffff7f);
	}
	else
	{
		K_CP15_C1_OP_SET(0x80);
	}
}

/**
 * @BRIEF	: Enable/Disable DCache(Data Cache)
 * @PARAM	: b			0 disable; 1 enable
 * @RETRUN	: void
 */
void k_cp5_c1_DCache_enablish(unsigned int b)
{
	if(b)
	{
		K_CP15_C1_OP_SET(0x4);
	}
	else
	{
		K_CP15_C1_OP_CLEAR(0xfffffff3)
	}
}

/**
 * @BRIEF	: Enable/Disable MMU
 * @PARAM	: b			0 disable; 1 enable
 * @RETRUN	: void
 */
void k_cp15_c1_MMU_enablish(unsigned int b)
{
	if(b)
	{
		K_CP15_C1_OP_SET(0x1);
		// should nop 4 times,waiting MMU started correctly
		__asm__("nop\n\t"	\
		"nop\n\t"	\
		"nop\n\t"	\
		"nop\n\t"	\
		);
	}
	else
	{
		K_CP15_C1_OP_CLEAR(0xfffffffe);
	}
}

/***************************************************************************************
                                 CP15 Register 2 Operations

		Register c2 is the Translation Table Base Register (TTBR), for the base address
		of the first-level translation table.

****************************************************************************************/
/*
  	access instructions
	MRC p15, 0, <Rd>, c2, c0, 0; read TTBR
	MCR p15, 0, <Rd>, c2, c0, 0; write TTBR
*/

/**
 * @BRIEF	: Write FLPT to MMU
 * @PARAM	: phy_addr	physical address of first level page table
 * @RETRUN	: void
 */
void k_cp15_c2_write_flpt(unsigned int phy_addr)
{
	__asm__("MCR p15, 0, %0, c2, c0, 0\n\t"::"r"(phy_addr));
}

/**
 * @BRIEF	: Read FLPT from MMU
 * @PARAM	: void
 * @RETRUN	: phy_addr	physical address of first level page table
 */
unsigned int k_cp15_c2_read_flpt(void)
{
	unsigned int phy_addr;
	K_CP15_ASM_READ(phy_addr, "MRC p15, 0, R0, c2, c0, 0\n\t");
//	__asm__("MRC p15, 0, %0, c2, c0, 0\n\t":"=m"(phy_addr));
	return phy_addr;
}

/***************************************************************************************
                                 CP15 Register 3 Operations

		Domain Access Control Register!It has 16 domians each has two bits!

****************************************************************************************/
/*
  	access instructions
	MRC p15, 0, <Rd>, c3, c0, 0 ; read domain access permissions
	MCR p15, 0, <Rd>, c3, c0, 0 ; write domain access permissions
*/

/**
 * @BRIEF	: Read Domain Access Control Register
 * @PARAM	: void
 * @RETRUN	: value of Domain Access Control Register
 */
unsigned int k_cp15_c3_read_domain(void)
{
	unsigned int val;
	K_CP15_ASM_READ(val, "MRC p15, 0, R0, c3, c0, 0\n\t");
//	__asm__("MRC p15, 0, %0, c3, c0, 0\n\t":"=m"(val));
	return val;
}

/**
 * @BRIEF	: Write Domain Access Control Register
 * @PARAM	: val	domain register value
 * @RETRUN	: void
 */
void k_cp15_c3_write_domain(unsigned int val)
{
	__asm__("MCR p15, 0, %0, c3, c0, 0\n\t"::"r"(val));
}

/***************************************************************************************
                                 CP15 Register 5 Operations

		Register c5 accesses the Fault Status Registers (FSRs). The FSRs contain the
	source of the last instruction or data fault. The instruction-side FSR is intended
	for debug purposes only. The FSR is updated for alignment faults, and external
	aborts that occur while the MMU is disabled.

****************************************************************************************/
/*
  			Access Instructions
  	MRC p15, 0, <Rd>, c5, c0, 0 ;read DFSR
	MCR p15, 0, <Rd>, c5, c0, 0 ;write DFSR
	MRC p15, 0, <Rd>, c5, c0, 1 ;read IFSR(Debug Only)
	MCR p15, 0, <Rd>, c5, c0, 1 ;write IFSR(Debug Only)
 */

/**
 * @BRIEF	: Read data abort status register
 * @PARAM	: void
 * @RETRUN	: register value
 */
unsigned int k_cp15_c5_read_data_abort_status(void)
{
	unsigned int val;
	K_CP15_ASM_READ(val, "MRC p15, 0, R0, c5, c0, 0\n\t");
//	__asm__("MRC p15, 0, %0, c5, c0, 0\n\t":"=m"(val));
	return val;
}

/**
 * @BRIEF	: Read Instruction abort status register
 * @PARAM	: void
 * @RETRUN	: register value
 * @NOTE	: Only for Debug mode, Some maybe doesn't support!
 */
unsigned int k_cp15_read_ins_abort_status(void)
{
	unsigned int val;
	K_CP15_ASM_READ(val, "MRC p15, 0, R0, c5, c0, 1\n\t");
//	__asm__("MRC p15, 0, %0, c5, c0, 1\n\t":"=m"(val));
	return val;
}

/**
 * @BRIEF	: Write data abort status register
 * @PARAM	: val register value
 * @RETRUN	: void
 */
void k_cp15_c5_wirteData_abort_status(unsigned int val)
{
	__asm__("MCR p15, 0, %0, c5, c0, 0\n\t"::"r"(val));
}

/**
 * @BRIEF	: Write Instruction abort status register
 * @PARAM	: val register value
 * @RETRUN	: void
 * @NOTE	: Only for Debug mode, Some maybe doesn't support!
 */
void k_cp15_write_ins_abort_status(unsigned int val)
{
	__asm__("MCR p15, 0, %0, c5, c0, 1\n\t"::"r"(val));
}

/***************************************************************************************
                                 CP15 Register 6 Operations

		Register 6 is the Fault Address Register (FAR). This contains the MVA of the
	access being attempted when the last fault occurred. The FAR is only updated for 
	data faults,not for prefetch faults. 
	 (You can find the address for a prefetch fault in R14.)

****************************************************************************************/
/* Access Instructions */
/*
	MRC p15, 0, Rd, c6, c0, 0 ;read FAR data
	MCR p15, 0, Rd, c6, c0, 0 ;write FAR data
 */
 
/**
 * @BRIEF	: Write fault address
 * @PARAM	: val register value
 * @RETRUN	: void
 * @NOTE	: This function can be used for special!
 */
void k_cp15_c6_write_FAR(unsigned int val)
{
	__asm__("MCR p15, 0, %0, c6, c0, 0\n\t"::"r"(val));
}

/**
 * @BRIEF	: Read fault address
 * @PARAM	: val register value
 * @RETRUN	: void
 * @NOTE	: This function can be used for special!
 */
unsigned int k_cp15_c6_read_FAR(void)
{
	unsigned int val;
	K_CP15_ASM_READ(val, "MRC p15, 0, R0, c6, c0, 0\n\t");
//	__asm__("MRC p15, 0, %0, c6, c0, 0\n\t":"=m"(val));
	return val;
}

/****************************************************************************************
 * 						Cache Flush Reference
 * 	When change MMU table base address, (common in task schedule),should flush MMU table
 * Base Address!In order to make there is no dirty data or instructions or TLB in caches
 ****************************************************************************************/

/**
 * @BRIEF	: Flush Instruction Cache
 * @PARAM	: void
 * @RETRUN	: void
 */
void k_cp15_ICacheFlush(void)
{
  unsigned long dummy;
  __asm__("MCR p15, 0, %0, c7, c5, 0\n\t"::"r"(dummy));
}
 
/**
 * @BRIEF	: Flush Data Cache
 * @PARAM	: void
 * @RETRUN	: void
 */
void k_cp15_DCacheFlush(void)
{
  unsigned long dummy;
  __asm__("MCR p15, 0, %0, c7, c6, 0\n\t"::"r"(dummy));
}
 
/**
 * @BRIEF	: Flush Cache
 * @PARAM	: void
 * @RETRUN	: void
 */
void k_cp15_CacheFlush(void)
{
  unsigned long dummy;
  __asm__("MCR p15, 0, %0, c7, c7, 0\n\t"::"r"(dummy));
}

/**
 * @BRIEF	: Flush TLB Cache
 * @PARAM	: void
 * @RETRUN	: void
 */
void k_cp15_TLBFlush(void)
{
  unsigned long dummy;
  __asm__("MCR  P15, 0, %0, c8, c7, 0\n\t"::"r"(dummy));
}

/****************************************************************************************
 * 						MMU Switch Opreation
 * 	When change MMU table base address, (common in task schedule),should flush MMU table
 * Base Address!In order to make there is no dirty data or instructions or TLB in caches
 ****************************************************************************************/

/**
 * @BRIEF	: MMU Switch
 * @PARAM	: new_addr	new MMU base table address
 * @RETRUN	: old_addr	old MMU base table address
 */
unsigned int k_mmu_switch(unsigned int new_addr)
{
	unsigned int old_addr;
	
	// flush all the caches
	k_cp15_CacheFlush();
	k_cp15_TLBFlush();
	
	// switch MMU base table address
	old_addr = k_cp15_c2_read_flpt();
	k_cp15_c2_write_flpt(new_addr);
	
	return old_addr;
}

#endif /* _K_CP15_C_ */
